Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(autoware_path_generator): function to smooth the path #227

Merged
merged 78 commits into from
Mar 11, 2025
Merged

feat(autoware_path_generator): function to smooth the path #227

merged 78 commits into from
Mar 11, 2025
+501 −8

Conversation

sasakisasaki
Copy link
Contributor

@sasakisasaki sasakisasaki commented Feb 26, 2025 •
edited
Loading

Description

This is kind of feature porting from autoware.universe as follows

  • Import PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection from the autoware.universe side code
  • Also import all related functions from the autoware.universe side

⚠️ This PR needed to be merged with this launch side PR at the same time. ⚠️

Quality of Code

  • As there are some improvements needed, see this created issue for the detail of the plan for code quality improvement.
  • The unit test is added (by this commit).

The refactoring plan is mentioned in this issue.
Perhaps we can work on the refactoring with a few PRs. Please feel free to share your idea for the improvement of code quality. Thank you!

How was this PR tested?

We have two test strategies as follows.

Scenario Evaluator Tool Tests

Done as the following link (only can be seen via TIER IV internal)

Tests on Local Environment

  • I did vcs import at around 18:00 JST on 26th Feb. 2025
  • The executed commands are as follows:
$ git clone https://github.com/autowarefoundation/autoware.git
$ cd autoware

# Before the build
$ ./setup-dev-env.sh -y

(Enter password for user)

(ADDED: Perhaps you need to do `sudo shutdown -r now` for applying NVIDIA driver update)

$ vcs import src < autoware.repos

$ vim autoware-nightly.repos

(Edited `autoware.core` part so it used my branch on forked)

$ vcs import src < autoware-nightly.repos
$ source ~/autoware/install/setup.bash
$ sudo apt update && sudo apt upgrade
$ rosdep update
$ rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

# Deploy the attached three files for the following place, respectively
# - src/launcher/autoware_launch/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/path_generator.param.yaml
# - src/launcher/autoware_launch/autoware_launch/launch/components/tier4_planning_component.launch.xml
# - src/universe/autoware.universe/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

behavior_planning_launch_xml.txt
tier4_planning_component_launch_xml.txt
path_generator_param_yaml.txt

# Now start colcon build
$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

# Checked by psim
$ ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit
Screencast_psim.webm

Status of Additional Tests

(ADDED) It seems the tests on the evaluator shows a failed scenario link to failed one. I'm now investigating the issue.

Notes for reviewers

Please do not enable auto merge as this PR needed to be merged with this launch side PR at the same time.
Please feel free to provide all the needed tests for merging this PR. I'm really happy for performing the tests 👍

Effects on system behavior

Needed to be investigated during the testing process.

@sasakisasaki
feat: function to smooth the route (see below)
d12367c 
  Description:
    This commit is kind of feature porting from `autoware.universe` as follows
      * Import `PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection` from the following `autoware.universe` code
          https://github.com/autowarefoundation/autoware.universe/blob/a0816b7e3e35fbe822fefbb9c9a8132365608b49/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp#L74-L104
      * Also import all related functions from the `autoware.universe` side

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@github-actions GitHub Actions
Copy link

github-actions bot commented Feb 26, 2025 •
edited
Loading

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

@sasakisasaki sasakisasaki added run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) component:planning Route planning, decision-making, and navigation. (auto-assigned) labels Feb 26, 2025
@sasakisasaki sasakisasaki self-assigned this Feb 26, 2025
@xmfcx xmfcx added the type:new-feature New functionalities or additions, feature requests. label Feb 28, 2025
@codecov Codecov
Copy link

codecov bot commented Feb 28, 2025 •
edited
Loading

Codecov Report

Attention: Patch coverage is 65.60000% with 43 lines in your changes missing coverage. Please review.

Project coverage is 10.38%. Comparing base (4cb18f5) to head (03a75c5).
Report is 75 commits behind head on main.

Files with missing lines Patch % Lines
planning/autoware_path_generator/src/utils.cpp 62.36% 21 Missing and 14 partials ⚠️
planning/autoware_path_generator/src/node.cpp 53.33% 4 Missing and 3 partials ⚠️
...ng/autoware_test_utils/src/autoware_test_utils.cpp 92.30% 1 Missing ⚠️
Additional details and impacted files 
@@             Coverage Diff             @@
##             main     #227       +/-   ##
===========================================
- Coverage   78.75%   10.38%   -68.37%     
===========================================
  Files          11      121      +110     
  Lines         193    10272    +10079     
  Branches       73     1680     +1607     
===========================================
+ Hits          152     1067      +915     
- Misses         11     8901     +8890     
- Partials       30      304      +274
Flag Coverage Δ
differential 10.38% <65.60%> (?)
total ?

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:
  • ❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

@sasakisasaki
Merge branch 'main' into feat-embed-smooth-path-as-alpha-quality
f1f499a 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
bugs: fix remaining conflicts
17054c9 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
kosuke55
kosuke55 reviewed Feb 28, 2025
View reviewed changes
sasakisasaki and others added 7 commits February 28, 2025 18:11
@sasakisasaki @kosuke55
Update planning/autoware_path_generator/src/utils.cpp
5a2766e 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
@sasakisasaki @kosuke55
Update planning/autoware_path_generator/src/utils.cpp
808fb5d 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
@sasakisasaki
refactor: as follows
19185eb 
  * Enhance error handlings
  * Remove unused variables
  * Simplify the code
  * Improve readability a little bit

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
Merge branch 'feat-embed-smooth-path-as-alpha-quality' of github.com:…
fc90ae4 
…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@pre-commit-ci
style(pre-commit): autofix
c2e91f9
@sasakisasaki
refactor: enhance error handling
5945ef7 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@pre-commit-ci
style(pre-commit): autofix
c4b3500
sasakisasaki added a commit to sasakisasaki/autoware_launch that referenced this pull request Mar 4, 2025
@sasakisasaki
feat(path_generator): add parameters (see below)
e1fabc8 
  * This fix is for the following PR:
      autowarefoundation/autoware.core#227

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki sasakisasaki mentioned this pull request Mar 4, 2025
Merged
@sasakisasaki
bug: fix wrong function declaration in header
ace5fbe 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
bug: fix wrong point index calculation
0dee050 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
bug: remove meaningless comment
b6a8492 
  * This comment is wrote because of my misunderstanding

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
fix: apply pre-commit
ea3bfed 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
fix: smooth path before cropping trajectory points
a322d4d 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki sasakisasaki mentioned this pull request Mar 4, 2025
Open
13 tasks
@sasakisasaki
Copy link
Contributor Author

Fixing error in some CI checks.

@sasakisasaki
bug: fix shadow variable
865b38f 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
Merge branch 'feat-embed-smooth-path-as-alpha-quality' of github.com:…
8318cf1 
…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
bug: sorry, this is unsaved fix, ...
31875a9 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@kosuke55
Copy link
Contributor

I confirmed it works!

image

image

@sasakisasaki
cosmetic: fix wrong comment
d1f52ae 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
bug: unused function get_goal_lanelet() remaining
c95effc 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
Copy link
Contributor Author

@kosuke55 Now the final tests are running.
https://evaluation.tier4.jp/evaluation/reports/9c281bdd-e333-578f-b7ca-09ed7e7dee82?project_id=prd_jt

@sasakisasaki
bug: carefully handle the pre goal velocity
b12c40c 
  * It seems zero pre goal velocity makes scenario fail
      - We need to insert appropriate velocity for pre goal

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
@sasakisasaki
Copy link
Contributor Author

@kosuke55 Now the final tests are running. https://evaluation.tier4.jp/evaluation/reports/9c281bdd-e333-578f-b7ca-09ed7e7dee82?project_id=prd_jt

Sorry, the final tests failed. Now let me put the final of final test results here:

sasakisasaki and others added 3 commits March 10, 2025 19:30
@sasakisasaki @kosuke55
Update planning/autoware_path_generator/src/utils.cpp
c34c359 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
@sasakisasaki @kosuke55
Update planning/autoware_path_generator/src/utils.cpp
4938af7 
Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
@pre-commit-ci
style(pre-commit): autofix
03a75c5
@sasakisasaki sasakisasaki requested a review from kosuke55 March 10, 2025 10:37
@sasakisasaki
Copy link
Contributor Author

Now ready 👍

kosuke55
kosuke55 approved these changes Mar 10, 2025
View reviewed changes
Copy link
Contributor

@kosuke55 kosuke55 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM!

( not tested after #227 (comment) )

@sasakisasaki
Copy link
Contributor Author

I'm doing the double check purpose tests as this 👍. I'm going to merge this PR (with launch side together) after seeing the test results.

@kosuke55
Copy link
Contributor

@yhisaki @soblin
could you review this PR as code owner?

@sasakisasaki
Copy link
Contributor Author

Now test result on the evaluator looks good.

@sasakisasaki
Copy link
Contributor Author

Now let me merge with this launch side PR

sasakisasaki added a commit to autowarefoundation/autoware_launch that referenced this pull request Mar 11, 2025
@sasakisasaki @kosuke55
feat(path_generator): add parameters (#1343)
cea7f5a 
* feat(path_generator): add parameters (see below)

  * This fix is for the following PR:
      autowarefoundation/autoware.core#227

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* Update autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/path_generator/path_generator.param.yaml

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

---------

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
@sasakisasaki sasakisasaki merged commit 7513d02 into autowarefoundation:main Mar 11, 2025
26 of 29 checks passed
SakodaShintaro pushed a commit to tier4/autoware.core that referenced this pull request Mar 11, 2025
@sasakisasaki @pre-commit-ci
@kosuke55 @mitukou1109 @SakodaShintaro
feat(autoware_path_generator): function to smooth the path (autowaref…
6f4f084 
…oundation#227)

* feat: function to smooth the route (see below)

  Description:
    This commit is kind of feature porting from `autoware.universe` as follows
      * Import `PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection` from the following `autoware.universe` code
          https://github.com/autowarefoundation/autoware.universe/blob/a0816b7e3e35fbe822fefbb9c9a8132365608b49/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp#L74-L104
      * Also import all related functions from the `autoware.universe` side

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bugs: fix remaining conflicts

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* refactor: as follows

  * Enhance error handlings
  * Remove unused variables
  * Simplify the code
  * Improve readability a little bit

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* refactor: enhance error handling

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug: fix wrong function declaration in header

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix wrong point index calculation

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: remove meaningless comment

  * This comment is wrote because of my misunderstanding

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix: apply `pre-commit`

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix: smooth path before cropping trajectory points

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix shadow variable

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix missing parameters for `autoware_path_generator`

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix by cpplint

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug: apply missing fix proposed by cpplint

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug: `autoware_test_utils` should be in the `test_depend`

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix(autoware_path_generator): add maintainer and author

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* fix: by pre-commit

  * Sorry, I was forgetting to do this on my local env.

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix: smooth path only when a goal point is included

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: do error handling

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug: fix wrong distance calculation

  * The goal position is generally separate from the path points

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix: remove sanity check temporary as following reasons

  * CI (especially unit tests) fails due to this sanity check
  * As this is out of scope for this PR, we will fix the bug
    where the start and end are reversed in another PR

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* refactor: fix complexity

  * We should start from the simple one
  * Then we can add the necessary optimization later

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: missing fixes in the include header

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: inconsistent function declaration

  * The type of returned value and arguments were wrong

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* Update planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/autoware_path_generator/src/node.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* style(pre-commit): autofix

* fix: apply comment in the following PR

  * autowarefoundation#227 (comment)

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix: sorry, I was missing one comment to be applied

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug: fix wrong goal point interpolation

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* feat: add test case (goal on left side)

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix as follows

  * Prevent name duplication (path_up_to_just_before_pre_goal)
  * Fix missing left/right bound
  * Goal must have zero velocity
  * Improve readability
  * Other minor fixes

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix duplicated zero velocity set

  * Zero velocity is set after the removed lines by this commit

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* feat: add one test case (goal on left side)

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* fix: apply comment from reviewer

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix(package.xml): update maintainer for the following packages

  * `autoware_planning_test_manager`
  * `autoware_test_utils`

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* Update planning/autoware_path_generator/src/node.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com>

* bug: fix missing header in the path

  * This finally causes an issue that the vehicle cannot engage

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix an issue that smooth connection does not work

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* refactor: simplify code

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: fix wrong pose at the goal (see below)

  * If we return nullopt here, the original path
    whose goal position is located at the center line is used.
  * Unless far from the goal point, the path becomes smoothed one
    whose goal position is located at the side of road correctly.
  * But as the goal approaches very closely, the goal position is
    shifted from smoothed one to the original one
  * Thus, the goal pose finally becomes wrong due to the goal position shift

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* refactor: no need this line here

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug: fix so we follow the provided review comments

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: sorry, this is unsaved fix, ...

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* cosmetic: fix wrong comment

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: unused function `get_goal_lanelet()` remaining

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug: carefully handle the pre goal velocity

  * It seems zero pre goal velocity makes scenario fail
      - We need to insert appropriate velocity for pre goal

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Co-authored-by: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com>
@SakodaShintaro SakodaShintaro mentioned this pull request Mar 11, 2025
Merged
4 tasks
SakodaShintaro pushed a commit to tier4/autoware_launch that referenced this pull request Mar 11, 2025
@sasakisasaki @kosuke55 @SakodaShintaro
feat(path_generator): add parameters (autowarefoundation#1343)
28155cb 
* feat(path_generator): add parameters (see below)

  * This fix is for the following PR:
      autowarefoundation/autoware.core#227

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* Update autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/path_generator/path_generator.param.yaml

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

---------

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
SakodaShintaro added a commit to tier4/autoware.core that referenced this pull request Mar 11, 2025
@SakodaShintaro @sasakisasaki
@pre-commit-ci @kosuke55 @mitukou1109
feat(autoware_path_generator): function to smooth the path (autowaref…
7a61c59 
…oundation#227) (#9)

* feat: function to smooth the route (see below)

  Description:
    This commit is kind of feature porting from `autoware.universe` as follows
      * Import `PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection` from the following `autoware.universe` code
          https://github.com/autowarefoundation/autoware.universe/blob/a0816b7e3e35fbe822fefbb9c9a8132365608b49/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp#L74-L104
      * Also import all related functions from the `autoware.universe` side



* style(pre-commit): autofix

* bugs: fix remaining conflicts



* Update planning/autoware_path_generator/src/utils.cpp





* Update planning/autoware_path_generator/src/utils.cpp





* refactor: as follows

  * Enhance error handlings
  * Remove unused variables
  * Simplify the code
  * Improve readability a little bit



* style(pre-commit): autofix

* refactor: enhance error handling



* style(pre-commit): autofix

* bug: fix wrong function declaration in header



* bug: fix wrong point index calculation



* bug: remove meaningless comment

  * This comment is wrote because of my misunderstanding



* fix: apply `pre-commit`



* fix: smooth path before cropping trajectory points



* bug: fix shadow variable



* bug: fix missing parameters for `autoware_path_generator`



* bug: fix by cpplint



* style(pre-commit): autofix

* bug: apply missing fix proposed by cpplint



* style(pre-commit): autofix

* bug: `autoware_test_utils` should be in the `test_depend`



* fix(autoware_path_generator): add maintainer and author



* style(pre-commit): autofix

* fix: by pre-commit

  * Sorry, I was forgetting to do this on my local env.



* fix: smooth path only when a goal point is included



* bug: do error handling



* style(pre-commit): autofix

* bug: fix wrong distance calculation

  * The goal position is generally separate from the path points



* fix: remove sanity check temporary as following reasons

  * CI (especially unit tests) fails due to this sanity check
  * As this is out of scope for this PR, we will fix the bug
    where the start and end are reversed in another PR



* refactor: fix complexity

  * We should start from the simple one
  * Then we can add the necessary optimization later



* bug: missing fixes in the include header



* bug: inconsistent function declaration

  * The type of returned value and arguments were wrong



* Update planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp





* Update planning/autoware_path_generator/src/node.cpp





* Update planning/autoware_path_generator/src/utils.cpp





* Update planning/autoware_path_generator/src/utils.cpp





* style(pre-commit): autofix

* fix: apply comment in the following PR

  * autowarefoundation#227 (comment)



* fix: sorry, I was missing one comment to be applied



* style(pre-commit): autofix

* bug: fix wrong goal point interpolation



* feat: add test case (goal on left side)



* bug: fix as follows

  * Prevent name duplication (path_up_to_just_before_pre_goal)
  * Fix missing left/right bound
  * Goal must have zero velocity
  * Improve readability
  * Other minor fixes



* bug: fix duplicated zero velocity set

  * Zero velocity is set after the removed lines by this commit



* feat: add one test case (goal on left side)



* Update planning/autoware_path_generator/src/utils.cpp





* fix: apply comment from reviewer



* fix(package.xml): update maintainer for the following packages

  * `autoware_planning_test_manager`
  * `autoware_test_utils`



* Update planning/autoware_path_generator/src/node.cpp





* Update planning/autoware_path_generator/src/utils.cpp





* Update planning/autoware_path_generator/src/utils.cpp





* bug: fix missing header in the path

  * This finally causes an issue that the vehicle cannot engage



* bug: fix an issue that smooth connection does not work



* refactor: simplify code



* bug: fix wrong pose at the goal (see below)

  * If we return nullopt here, the original path
    whose goal position is located at the center line is used.
  * Unless far from the goal point, the path becomes smoothed one
    whose goal position is located at the side of road correctly.
  * But as the goal approaches very closely, the goal position is
    shifted from smoothed one to the original one
  * Thus, the goal pose finally becomes wrong due to the goal position shift



* refactor: no need this line here



* style(pre-commit): autofix

* bug: fix so we follow the provided review comments



* bug: sorry, this is unsaved fix, ...



* cosmetic: fix wrong comment



* bug: unused function `get_goal_lanelet()` remaining



* bug: carefully handle the pre goal velocity

  * It seems zero pre goal velocity makes scenario fail
      - We need to insert appropriate velocity for pre goal



* Update planning/autoware_path_generator/src/utils.cpp





* Update planning/autoware_path_generator/src/utils.cpp





* style(pre-commit): autofix

---------

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Co-authored-by: Junya Sasaki <j2sasaki1990@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Co-authored-by: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@soblin soblin soblin approved these changes

@kosuke55 kosuke55 kosuke55 approved these changes

@satoshi-ota satoshi-ota Awaiting requested review from satoshi-ota satoshi-ota is a code owner

@takayuki5168 takayuki5168 Awaiting requested review from takayuki5168 takayuki5168 is a code owner

@JianKangEgon JianKangEgon Awaiting requested review from JianKangEgon JianKangEgon is a code owner

@kyoichi-sugahara kyoichi-sugahara Awaiting requested review from kyoichi-sugahara kyoichi-sugahara is a code owner

@TakaHoribe TakaHoribe Awaiting requested review from TakaHoribe TakaHoribe is a code owner

@yhisaki yhisaki Awaiting requested review from yhisaki yhisaki is a code owner

@zulfaqar-azmi-t4 zulfaqar-azmi-t4 Awaiting requested review from zulfaqar-azmi-t4 zulfaqar-azmi-t4 is a code owner

@youtalk youtalk Awaiting requested review from youtalk

@mitukou1109 mitukou1109 Awaiting requested review from mitukou1109

Assignees

@sasakisasaki sasakisasaki

@kosuke55 kosuke55

Labels
component:planning Route planning, decision-making, and navigation. (auto-assigned) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) type:new-feature New functionalities or additions, feature requests.
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
6 participants
@sasakisasaki @kosuke55 @youtalk @soblin @mitukou1109 @xmfcx